#include <iostream>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include "../inc/XYGrid.h"
#include "../inc/PointCloud.h"
#include "../inc/LoaderOFF.h"
#include "../inc/Tools.h"
#include "../inc/HoughFrame.h"
#include "../inc/HoughSpace.h"

#ifdef __APPLE__
        #include <OpenGL/gl.h>
        #include <OpenGL/glu.h>
        #include <GLUT/glut.h>
#else
        #ifdef _WIN32
        #include "../glut.h"
        #else
        #include <GL/gl.h>
        #include <GL/glu.h>
        #include <GL/glut.h>
        #endif
#endif

using namespace std;

/* ************************************************************************ */
/* ***						VARIABLES GLOBALES							*** */
/* ************************************************************************ */
int			__winX = 800;								/* Largeur de la fenetre glut */
int			__winY = 600;								/* Hauteur de la fenetre glut */

int			__angle_x = -70, __angle_z = 180;				/* Orientation de la caméra */
int			__mouse_x = 0, __mouse_y = 0;				/* Position de la souris */
		
int			__xFrameToDisplay = 0;						/* Abscisse de la case de la grille courante */
int			__yFrameToDisplay = 0;						/* Ordonnee de la case de la grille courante */

float		__PointOfViewX = 0;							/* Abscisse de la camera */
float		__PointOfViewY = 0;							/* Ordonnée de la camera */
float		__PointOfViewZ = 0;							/* Hauteur de la camera */
float		__lookAtX = 1;								/* Abscisse du point de visee */
float		__lookAtY = 0;								/* Ordonnée du point de visée */
float		__lookAtZ = 0;								/* Hauteur du point de visee */

PointCloud	__data;										/* Le nuage de points donnée charge en memoire */
float		__xMin, __yMin, __xMax;						/* Coordonnées du point inferieur gauche de la boite englobante du nuage */
float		__yMax, __zMin, __zMax;						/* Coordonnées du point superieur droit de la boite englobante du nuage */
	
XYGrid		__grid;										/* Grille dans laquelle le nuage de points va etre plongé */
int			__resX, __resY;								/* Resolution de(s) la(es) grille(s) dans laquelle plonger le nuage de points. i.e. son nombre de case selon x et y */
	
float		__rMax;										/* Rayon maximum des cercles que l'on recherche, tous les cercles plus grands seront ignores */

float		__seuilBas, __seuilHaut;					/* Hauteur entre les quelles sera effectuee la coupe */

float		__resHoughA, __resHoughB, __resHoughR;		/* Resolutions en a,b et r de l'espace de Hough local */
int			__thresh;									/* Seuil au dessus duquel une case de l'espace de Hough est considérée comme représentative */

vector		<Circle> __trunks;							/* Tableau de cercle ou seront stockes les cercles representants les troncs */
vector		<float> __trunksZ;							/* Tableau ou l'on stockera la hauteur ou les troncs ont ete trouves */
bool*		__conflits;									/*Tableau de booleen : pour chaque cercle trouvé, il vaut vrai si ce cercle est en conflit avec un autre */
int			__nbConflits = 0;							/* Nombre de cercles qui sont en conflits */
int			__nbInscrits = 0;							/* Nombre de cercles qui sont inscrits ou qui en contiennent */

bool		__displayGrid = true;						/* Affichage ou non de la grille */
bool		__displayCloud = false;						/* Affichage ou non du nuagede points global */
bool		__displayFrameCloud = false;				/* Affichage ou non du nuage de points de la case actuelle */

/* Ces tableaux servent pour resoudre les conflits */
int				__nbEtiquette;
Circle*			__tabCircles;
int*			__etiquette;
int*			__nbCircles;
vector<Circle>	__circleToDisplay;
int				__cpt = 0;

bool			__displayConflits = true;
bool			__conflictsSolved = false;

/* ************************************************************************ */
/* ***					PROTOTYPES DES FONCTIONS							*** */
/* ************************************************************************ */

void	getPointOfView ( int _xFrameToDisplay, int _yFrameToDisplay );
void	keyboardGL(unsigned char _k, int _x, int _y);
void	keyboardSpecialGL(int _k, int _x, int _y);
void	mouseGL (int _button, int _state, int _x, int _y);
void	motionGL (int _x, int _y);
void	displayGL();
void	reshapeGL(int _w, int _h);
void	initGL();
int		main(int _argc, char ** _argv);

/* ************************************************************************ */
/* ***						CORPS DU PROGRAMME							*** */
/* ************************************************************************ */
                
void getPointOfView ( int _xFrameToDisplay, int _yFrameToDisplay )
{
        Point bot = __grid.getFrameBotPoint( _xFrameToDisplay, _yFrameToDisplay);
        Point top = __grid.getFrameTopPoint( _xFrameToDisplay, _yFrameToDisplay );
        
        __PointOfViewX = bot.getX() + (top.getX() - bot.getX()) /2;
        __PointOfViewY = bot.getY() - 10;
		__PointOfViewZ = __zMin + 3;
        
        __lookAtX = bot.getX() + (top.getX() - bot.getX()) /2;
        __lookAtY = bot.getY() + (top.getY() - bot.getY()) /2;
		__lookAtZ = __zMin;
		
		reshapeGL( __winX, __winY);
}

void keyboardGL(unsigned char _k, int _x, int _y)
{
        if(_k == 27 || _k == 'q' || _k == 'Q')
		{
			/* On quitte l'application */
			exit(0);        
		}

		if ( _k == 'g' || _k == 'G')
        {
			/* On active ou désactive l'affichage de la grille */
			if(__displayGrid)
				__displayGrid = false;
			else
				__displayGrid = true;
		}

		if ( _k == 'c' || _k == 'C')
        {
			/* On active ou désactive l'affichage du nuage de points */
			if(__displayCloud)
				__displayCloud = false;
			else
				__displayCloud = true;
		}

		if ( _k == 'f' || _k == 'F')
        {
			/* On active ou désactive l'affichage du nuage de points de la case*/
			if(__displayFrameCloud)
				__displayFrameCloud = false;
			else
				__displayFrameCloud = true;
		}

		if ( _k == 'h' || _k == 'H')
        {
			/* Aide */
			cout<<"**************** Aide ****************"<<endl<<endl;
			cout<<"Flèches: se deplacer sur la grille"<<endl;
			cout<<"Espace: lance la détection des troncs"<<endl;
			cout<<"G: active/désactive l’affichage de la grille"<<endl;
			cout<<"C: active/désactive l’affichage du nuage de point global"<<endl;
			cout<<"F: active/désactive l’affichage du nuage de points de la case courante"<<endl<<endl;
			cout<<"R: active/désactive la résolution des conflits"<<endl;
			cout<<"**************************************"<<endl<<endl;
		}
        
        if ( _k == ' ')
        {
			/* On vide le tableau __conflits et le tableau __trunks et on reinitialise le __nbConflits et __nbInscrits */
			free(__conflits);
			__trunks.clear();
			
			/* On lance le traitement sur le nuage de points */
			for ( int i = 0 ; i < __grid.getNbFrameX() ; i++ )
			{
				for ( int j = 0 ; j < __grid.getNbFrameY() ; j++ )
				{
					if ( __grid.getSizeOfFrameCloud(i,j) >= 3 )
					{						
						/* Utile pour afficher les cercles a la bonne hauteur */
						int nbTrunks = (int)__trunks.size();
						float hauteur = __grid.getFrameZMin(i,j);
						
						/* Ces deux points définissent la boite englobante de l'espace de Hough local */
						Point bot ( (i*__grid.getFrameWidth()) - (2*__rMax) + __xMin, (j*__grid.getFrameHeight()) - (2*__rMax) + __yMin, 0, 0,0,0 );
						Point top ( ((i+1)*__grid.getFrameWidth()) + (2*__rMax) + __xMin, ((j+1)*__grid.getFrameHeight()) + (2*__rMax) + __yMin, __rMax, 0,0,0 );
						
						/* On déclare et on initialise l'espace de Hough aux bonnes dimensions */
						HoughSpace hough ( RESOLUTION, bot, top, __resHoughA, __resHoughB, __resHoughR);
						
						/* On remplit l'espace de Hough avec les cercles trouves dans la case [i,j] de la grille */
						__grid.getFrameCircles( hough, __rMax, i, j );
						
						/* On calcule le pourcentage de seuil en fonction de la case 
						int coeff = ((1.0/hough.getScoreTotal())*100) + 1; */
						
						/* On seuille cet espace de hough et on ajoute les cercles representatifs dans le tableau de cercle resultat __trunks */
						hough.threshold( __trunks, (int)__thresh );
						
						for ( int k = 0 ; k < (int)__trunks.size() - nbTrunks ; k++ )
							__trunksZ.push_back( hauteur + __seuilBas );
					}
				}
			}
			
			/* On repere ensuite les cercles qui se coupent ou qui se contiennent les uns les autres */
			__conflits = (bool*) calloc ( __trunks.size(), sizeof ( bool ) );
			
			for ( int i = 0 ; i < (int)__trunks.size()-1 ; i++ )
			{               
					for ( int j = 0 ; j < (int)__trunks.size() && __conflits[i] == false ; j++ )
					{
							if ( j != i )
							{
									__conflits[i] = __trunks[i].intersect(__trunks[j]);
									
									if ( __conflits[i] )
										__nbConflits++;
							}
							
							if ( __trunks[i].conflict(__trunks[j]) == 0 )
								__nbInscrits++;
					}
			}
			
			cout<<"**************** Resultats ****************"<<endl<<endl;
			cout<<__trunks.size()<<" cercles trouves"<<endl;
			cout<<__nbConflits<<" conflits trouves"<<endl;
			cout<<"dont "<<__nbInscrits<<" cercles inscrits"<<endl<<endl;
			cout<<"*******************************************"<<endl;
        }
		
		if ( _k == 'r' || _k == 'R' )
		{
			if ( __conflictsSolved == false )
			{
				/* Pour éliminer les cercles qui sont inscrits les uns dans les autres, on ne garde que le cercle moyen de ceux-ci */
				__etiquette = (int*) malloc(__trunks.size() * sizeof(int));
				for(int i=0; i<(int)__trunks.size(); i++)
						__etiquette[i] = -1;
				
				for ( int i = 0 ; i < (int)__trunks.size()-1 ; i++ )
				{                                       
						for ( int j = i+1 ; j < (int)__trunks.size(); j++ )
						{
								if(__trunks[i].conflict(__trunks[j]) == 0)
								{
										if(__etiquette[i] == -1 && __etiquette[j] == -1)
										{
												__etiquette[i] = __nbEtiquette;
												__etiquette[j] = __nbEtiquette;
												__nbEtiquette ++;
										}
										else
										{
												if(__etiquette[i] != -1)
														__etiquette[j] = __etiquette[i];
												else if(__etiquette[j] != -1)
														__etiquette[i] = __etiquette[j];
										}
								}
						}
						
						if ( __etiquette[i] == -1 )
						{
								__circleToDisplay.push_back(__trunks[i]);
								__cpt++;
						}
				}
				
				__tabCircles =  (Circle*) malloc(__nbEtiquette * sizeof(Circle));
				__nbCircles = (int*) calloc(__nbEtiquette, sizeof(int));
		
				for ( int i = 0 ; i < __nbEtiquette ; i++ )
						__tabCircles[i].setABR(0,0,0);
				
				for ( int i = 0 ; i < (int)__trunks.size() ; i++ )
				{
						__nbCircles[__etiquette[i]] ++;
						__tabCircles[__etiquette[i]].setABR(__tabCircles[__etiquette[i]].getA() + __trunks[i].getA(), __tabCircles[__etiquette[i]].getB() + __trunks[i].getB(), __tabCircles[__etiquette[i]].getR() + __trunks[i].getR());
				}
				
				for ( int i = 0 ; i < __nbEtiquette ; i++ )
				{
						__tabCircles[i].setABR(__tabCircles[i].getA()/(float)__nbCircles[i], __tabCircles[i].getB()/(float)__nbCircles[i], __tabCircles[i].getR()/(float)__nbCircles[i]);
						__circleToDisplay.push_back(__tabCircles[i]);
				}
				
				__conflictsSolved = true;
			}
			
			if ( __displayConflits )
				__displayConflits = false;
			
			else
				__displayConflits = true;
		}
		
        glutPostRedisplay();
}

void keyboardSpecialGL(int _k, int _x, int _y)
{
        if ( _k == GLUT_KEY_RIGHT && __xFrameToDisplay < __grid.getNbFrameX()-1 )
                __xFrameToDisplay++;
        
        if ( _k == GLUT_KEY_LEFT && __xFrameToDisplay > 0 )
                __xFrameToDisplay--;
        
        if ( _k == GLUT_KEY_UP && __yFrameToDisplay < __grid.getNbFrameY()-1 )
                __yFrameToDisplay++;
        
        if ( _k == GLUT_KEY_DOWN && __yFrameToDisplay > 0 )
                __yFrameToDisplay--;
        
        getPointOfView( __xFrameToDisplay, __yFrameToDisplay);
        
        glutPostRedisplay();
}

void mouseGL (int _button, int _state, int _x, int _y)
{
	switch (_button)
     {
		 case GLUT_LEFT_BUTTON: 
		 	if(_state==GLUT_DOWN)
		 	{
				__mouse_x = _x;
				__mouse_y = _y;
			}
		 break;

	}
	glutPostRedisplay();
}

void motionGL (int _x, int _y)
{
	__angle_x += _y -__mouse_y;	
	__angle_z += _x -__mouse_x;
	__mouse_x = _x;
	__mouse_y = _y;

	glutPostRedisplay();
}

void displayGL()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	
	/* On positione le point de vue avec la trackball */
	glLoadIdentity();
	glRotatef(__angle_x, 1,0,0);
	glRotatef(__angle_z, 0,0,1);
	
	glPushMatrix();
	
	glTranslatef(__PointOfViewX, __PointOfViewY, __PointOfViewZ);
	
	if ( !__displayConflits )
    {
		   for ( int i = 0 ; i < __cpt ; i++ )
				   __circleToDisplay[i].draw(__zMin+__seuilBas,0,0,1);
		   
		   for(int i = __cpt; i < (int)__circleToDisplay.size(); i++)
				   __circleToDisplay[i].draw(__zMin + __seuilBas,0,1,0);
	}

	if ( __displayConflits )
	{
		if ( (int)__trunks.size() > 0 )
		{
			for ( int i = 0 ; i < (int)__trunks.size() ; i++ )
			{
				if ( __conflits[i] )
					__trunks[i].draw( __zMin + __seuilBas, 1, 0, 0 );
				
				else
					__trunks[i].draw( __zMin + __seuilBas, 0, 0, 1 );
			}
		}
	}
	
	if(__displayGrid)
	{
		/* On affiche la grille en couleur */
		glColor3f(0,0,0);
		__grid.displayGrid( __zMin );
	}
	
	if(__displayCloud)
	{
		/* On affiche le nuage de points en couleur */
		__data.displayPointCloud(0.7, 0.5, 0.2);
	}

	if(__displayFrameCloud)
	{
		/* On affiche le nuage de points de la case en couleur */
		glColor3f(0.7, 0.5, 0.2);
		__grid.displayIJCloud(__xFrameToDisplay, __yFrameToDisplay);
	}

	glPopMatrix();
	
	glutSwapBuffers();
}

void reshapeGL(int _w, int _h)
{
       __winX = _w;
       __winY = _h;

        glViewport(0, 0,__winX,__winY);
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();

        gluPerspective(60.0, (GLfloat)_w/(GLfloat)_h, 0.1, 500.0);

        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt( __PointOfViewX, __PointOfViewY, __PointOfViewZ, __lookAtX, __lookAtY, __lookAtZ, 0, 1, 0);
                
        glutPostRedisplay();
}

void initGL()
{
        GLfloat mat_specular   [4] = { 0.18, 0.18, 0.18, 0.18 };
        GLfloat mat_shininess  [ ] = { 128 };
        GLfloat global_ambient [ ] = { 0.02, 0.02, 0.05, 0.05 };
        GLfloat light0_ambient [ ] = { 0, 0, 0, 0 };
        GLfloat light0_diffuse [ ] = { 0.85, 0.85, 0.8, 0.85 };
        GLfloat light0_specular[ ] = { 0.85, 0.85, 0.15, 0.85 };

        glClearColor(0.8, 0.8, 0.7, 1.0);

        glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
        glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, mat_specular);
        glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, mat_shininess);

        glLightfv(GL_LIGHT0, GL_AMBIENT, light0_ambient);
        glLightfv(GL_LIGHT0, GL_DIFFUSE, light0_diffuse);
        glLightfv(GL_LIGHT0, GL_SPECULAR, light0_specular);
        glLightModelfv(GL_LIGHT_MODEL_AMBIENT, global_ambient);

        glEnable(GL_COLOR_MATERIAL);
        glEnable(GL_NORMALIZE);

        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

        glEnable(GL_DEPTH_TEST);
        glDepthFunc(GL_LEQUAL);
}

int main(int _argc, char ** _argv)
{
		int posX, posY;					/* Position initiale de la fenetre glut a l'ecran */
		int nbPoints;					/* Nombre de points que devra contenir la coupe */
		
		/* Recuperation des arguments passes dans la console */
		if ( _argc < 12 )
		{
				cout<<endl<<"Usage : "<<_argv[0]<<" suivi des arguments suivants :\n\n- fichier a charger\n- resolution x de la grille\n- resolution y de la grille\n- Hauteur basse de la coupe\n- Hauteur haute de la coupe"<<endl;
				cout<<"- Nombre de points par coupe\n- Rayon maximum des troncs a chercher"<<endl;
				cout<<"- Resolution de l'espace de Hough sous forme \"a b r\""<<endl;
				cout<<"- Seuil de l'espace de Hough"<<endl;
				cout<<endl<<"Tous les arguments doivent etre presents"<<endl;
				return -1;
		}
		
		__resX = atoi(_argv[2]);
		__resY = atoi(_argv[3]);
		__seuilBas = atof(_argv[4]);
		__seuilHaut = atof(_argv[5]);
        nbPoints = atoi (_argv[6]);
		__rMax = atof(_argv[7]);
		__resHoughA = atof (_argv[8]);
		__resHoughB = atof (_argv[9]);
		__resHoughR = atof(_argv[10]);
		__thresh = atoi(_argv[11]);
		
		/* Initialisation de la fenetre glut */
        glutInit(&_argc, _argv);

        posX = (glutGet(GLUT_SCREEN_WIDTH ) -__winX) / 2;
        posY = (glutGet(GLUT_SCREEN_HEIGHT) -__winY) / 2;

        glutInitWindowSize(__winX,__winY);
        glutInitWindowPosition(posX, posY);

        glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
        glutCreateWindow("Methode Locale");

        glutDisplayFunc(displayGL);
        glutReshapeFunc(reshapeGL);

		glutKeyboardFunc(keyboardGL);
        glutSpecialFunc(keyboardSpecialGL);
		
		glutMouseFunc(mouseGL);
		glutMotionFunc(motionGL);
                
        initGL();
                
		/* Chargement du fichier .off contenant le nuage de points */
		if ( loadOffFile ( _argv[1], &__data,  &__xMin, &__yMin, &__zMin, &__xMax, &__yMax, &__zMax ) == -1 )
		{
				cout<<"Erreur de chargement du fichier \""<<_argv[1]<<"\""<<endl;
				return -1;
		}
		
		/* On plonge le nuage de points dans la grille */
		__grid.getGridFromCloud(__data, __xMin, __yMin, __xMax, __yMax, __resX, __resY );
		
		/* On ne garde qu'une coupe de la grille : les nbPoints premiers points entre les hauteur seuilBas et seuilHaut */
		__grid.keepSlice( __seuilBas, __seuilHaut, (int)nbPoints );
		
		/* On récupère un point de vue correct pour la visualisation */
		getPointOfView( __xFrameToDisplay, __yFrameToDisplay );
		
		cout<<endl<<"\tAppuyez sur 'h' pour obtenir de l'aide"<<endl<<endl;
		
        glutMainLoop();

        return 0;
}
